// mechatronics_final_project.c
// written by Ben, last modified today
//
/////////////////////////////////////////////////////////////////

//INCLUDES---------------------------------------------------------------------

// their includes
#include <avr/io.h> // included so we can do basic I/O
#include <avr/interrupt.h> // includes instruction library for interrupts and clocks
#include <util/delay.h> // included so we can call _delay_ms()
#include <stdio.h>	// make printf work
#include "serial.h"	// make USART work (eventually we don't include this)
#include "sonar.h"	// make sonar work

// our includes
#include "mae378_defines.h"	// defines
#include "mae378_utility.h"	// utility functions
#include "mae378_motors.h"	// motor functions
#include "mae378_sensors.h"	// sensor functions
//#include "mae378_debug.h"	// debug functions


//GLOBAL VARS-------------------------------------------------------------------

uint16_t volatile SonarReturnTime = 0;	// Grab variable from external sonar.h function
uint8_t volatile InPulse = 0;			// Declare variable from external sonar.h function
uint8_t volatile sonar_count = 0;		// Lets us run the sonar at 4Hz instead of 1000Hz
RobotState volatile currentState = ST_SEARCH;
RobotState volatile lastState = ST_SEARCH;
LiftState volatile liftState = LIFT_DOWN;
uint16_t volatile robotCounter = 0;
uint16_t volatile liftCounter = 0;



//FUNCTION PROTOTYPES----------------------------------------------------------

void init_timer();
void updateState();
void doAction();


//INTERRUPTS--------------------------------------------------------------------

ISR(USART_RXC_vect)	// USART Interrupt... Interacts with serial.h to receive data FROM the computer
{
	
}

ISR(SIG_OUTPUT_COMPARE0) //Timer running at 1,000 Hz
{

	// Update state as appropriate
	updateState();

	// Ok, now do something
	doAction();
	
	// Sonar (NOTE that LIGHT6 had to be disconnected to make this work
	sonar_count++;
	if(sonar_count==SONAR_COUNT_THRESHOLD) 
	{
		printf("\033[2J\033[H");						// Return cursor to home, clear screen
		printf("Time taken: %u\n\r",SonarReturnTime);	// Display latest 

		
		if (SonarReturnTime <= SONAR_DETECTION_DISTANCE) 					// I chose 300 as an arbitrary number to simply
			setPin(LIGHT1);								// detect if an object is in front
		else 
			clearPin(LIGHT1);
		
		PulseSonar();									// Run function from sonar.h
		sonar_count=0;
	}
		
	// QTI Sensors testing
	
	if (getInputPin(QTI_F) == ON) 
		setPin(LIGHT5);
	else
		clearPin(LIGHT5);

	if (getInputPin(QTI_M) == ON) 
		setPin(LIGHT6);
	else
		clearPin(LIGHT6);

	if (getInputPin(QTI_B) == ON) 
		setPin(LIGHT7);
	else
		clearPin(LIGHT7);

	// Wheel Sensors (NOTE that LIGHT0 and LIGHT1 had to be disconnected to 
	// make USART work)
	/*
	if(getInputPin(LWS_I) == ON)
		clearPin(LIGHT0);
	else
		setPin(LIGHT0);
	
	if(getInputPin(LWS_E) == ON)
		clearPin(LIGHT1);
	else
		setPin(LIGHT1);
	
	if(getInputPin(RWS_I) == ON)
		clearPin(LIGHT2);
	else
		setPin(LIGHT2);
	
	if(getInputPin(RWS_E) == ON)
		clearPin(LIGHT3);
	else
		setPin(LIGHT3);
	*/
}


// THE MAIN--------------------------------------------------------------------

int main()
{
	DDRA = OUTPUT;
	DDRB = INPUT;
	DDRC = OUTPUT;
	DDRD = OUTPUT;

	init_timer();
	sei();	
	init_uart();
	PulseSonar();


	while(1)
	{
		/*
		goForward();

		platformUp();
		delay(2000);
		platformDown();
		delay(2000);
		platformStop();
		delay(2000);
		*/
		

		/*
		clearPin(LIGHT7);
		setPin(LIGHT0);
		setPin(LIGHT1);
		setPin(LIGHT2);
		setPin(LIGHT3);
		setPin(LIGHT4);
		setPin(LIGHT5);
		setPin(LIGHT6);
		*/

		/*
		leftWheelForward();
		delay(1000);
		leftWheelBackward();
		delay(1000);
		stop();
		rightWheelForward();
		delay(1000);
		rightWheelBackward();
		delay(1000);
		stop();
		delay(5000);
		*/

		/*
		goForward();
		delay(1000);
		goBackward();
		delay(1000);
		turnLeft();
		delay(1000);
		turnRight();
		delay(1000);
		stop();
		delay(5000);
		*/
		

		/*
		setPin(LIGHT); // turn the light on

		delay(100);

		clearPin(LIGHT); // turn the light off
		*/		

		//delay(100);
		
	}
}


//FUNCTION DEFINITIONS---------------------------------------------------------

void init_timer()
{
	TCNT0=0;					  //clear the timer0 register
	OCR0=250;					  //set the timer0 compare value
	TCCR0=_BV(WGM01) | _BV(CS01) | _BV(CS00); //CTC on OCR0, clock/64
	TIMSK=_BV(OCIE0);				  //enable the timer0 interrupt
}

void updateState()
{
	RobotState sentinal = currentState;
	
	if(currentState == ST_DIE)
		return;
	else if(getInputPin(QTI_M) == LINE)
		currentState = ST_DIE;
	else if(getInputPin(QTI_F) == LINE)
	{
		currentState = ST_GETAWAYFROMSIDE_F;

		if(liftState != LIFT_DOWN)
			liftState = LIFT_MOVING_DOWN;
	}
	else if(getInputPin(QTI_B) == LINE)
	{
		currentState = ST_GETAWAYFROMSIDE_B;

		if(liftState != LIFT_DOWN)
			liftState = LIFT_MOVING_DOWN;
	}
	else
	{
		switch(currentState)
		{
			case ST_SEARCH:
			{
				if(SonarReturnTime <= SONAR_DETECTION_DISTANCE)
					currentState = ST_RUSH;

				if(liftState != LIFT_DOWN)
					liftState = LIFT_MOVING_DOWN;

				break;
			}
			case ST_RUSH:
			{
				if(SonarReturnTime > SONAR_DETECTION_DISTANCE)
					currentState = ST_SEARCH;
				else if(SonarReturnTime < SONAR_FLIP_DISTANCE)
					currentState = ST_CARRYOUT;

				if(liftState != LIFT_DOWN)
					liftState = LIFT_MOVING_DOWN;

				break;
			}
			case ST_FLIP:
			{
				if(SonarReturnTime > SONAR_DETECTION_DISTANCE)
					currentState = ST_SEARCH;
		
				if(liftState != LIFT_UP)
					liftState = LIFT_MOVING_UP;	
				else
					currentState = ST_CARRYOUT;	

				break;
			}
			case ST_CARRYOUT:
			{
				if(SonarReturnTime > SONAR_DETECTION_DISTANCE)
					currentState = ST_SEARCH;

				if(liftState != LIFT_UP)
					liftState = LIFT_MOVING_UP;	

				break;
			}
			case ST_GETAWAYFROMSIDE_F:
			{
				robotCounter++;
				if(robotCounter > BACKOFF_ITERATIONS)
				{
					currentState = ST_SEARCH;
					robotCounter = 0;
				}
				break;
			}
			case ST_GETAWAYFROMSIDE_B:
			{
				//if(DEBUG) printf("Iteration: %d\n", counter);
				robotCounter++;
				if(robotCounter > BACKOFF_ITERATIONS)
				{
					currentState = ST_SEARCH;
					robotCounter = 0;
				}
				break;
			}
			case ST_VICTORYDANCE:
			{
				// Don't need to change from here, because we only get here if 
				// we've won (theoretically).
				break;
			}
		}
	}

	if(currentState != sentinal)
		lastState = sentinal;

	switch(liftState)
	{
		case LIFT_DOWN:
		{

			break;
		}
		case LIFT_UP:
		{
			
			break;
		}
		case LIFT_MOVING_DOWN:
		{
			liftCounter--;
			if(liftCounter + LIFT_DOWN_OFFSET <= 0)
			{
				liftState = LIFT_DOWN;
				liftCounter = 0;
			}
			break;
		}
		case LIFT_MOVING_UP:
		{
			liftCounter++;
			if(liftCounter > LIFT_ITERATIONS)
			{
				liftState = LIFT_UP;
				//liftCounter = 0;
			}
			break;
		}
	}	

}

void doAction()
{
	if(currentState != ST_DIE)
	{
		clearPin(LIGHT_DEAD);
	}

	switch(currentState)
	{
		case ST_SEARCH:
		{
			turnLeft();
			break;
		}
		case ST_RUSH:
		{
			if(liftState == LIFT_DOWN)
				goForward();
			break;
		}
		case ST_FLIP:
		{
			stop();
			break;
		}
		case ST_CARRYOUT:
		{
			goForward();
			break;
		}
		case ST_GETAWAYFROMSIDE_F:
		{
			goBackward();
			break;
		}
		case ST_GETAWAYFROMSIDE_B:
		{
			goForward();
			break;
		}
		case ST_VICTORYDANCE:
		{
			// Don't need to change from here, because we only get here if 
			// we've won (theoretically).
			break;
		}
		case ST_DIE:
		{
			allStop();
			setPin(LIGHT_DEAD);
			break;
		}
	}

	if(currentState != ST_DIE)
	{
		switch(liftState)
		{
			case LIFT_DOWN:
			{
				platformStop();
				clearPin(LIGHT2);
				clearPin(LIGHT3);
				break;
			}
			case LIFT_UP:
			{
				platformStop();
				setPin(LIGHT2);
				setPin(LIGHT3);
				break;
			}
			case LIFT_MOVING_DOWN:
			{
				platformDown();
				setPin(LIGHT2);
				clearPin(LIGHT3);
				break;
			}
			case LIFT_MOVING_UP:
			{
				platformUp();
				clearPin(LIGHT2);
				setPin(LIGHT3);
				break;
			}
		}
	}
}
